
#ifndef ROBOT_SPLINE_HPP
#define ROBOT_SPLINE_HPP

#include "trajectory.hpp"

class Spline {
 public:
  Spline();
  ~Spline();
  
  void pushVia(cvec4d* pos);
  void insertVia(cvec4d* pos, int32_t idx);
  cvec4d* getVia(int32_t idx) { return vias->at(idx); }
  uint32_t numVias() { return vias->size(); }
  
  void planPath(Trajectory* trajectory, uint32_t timestep); // timestep in ms
 
 private:
  std::vector<cvec4d*>* vias;
};

inline Spline::Spline() {
  vias = new std::vector<cvec4d*>();
}

inline Spline::~Spline() {
  for(uint32_t i = 0; i < vias->size(); i ++) {
    delete vias->at(i);
  }
  delete vias;
}

inline void Spline::pushVia(cvec4d* pos) {
  vias->push_back(new cvec4d(pos));
}

inline void Spline::insertVia(cvec4d* pos, int32_t idx) {
  vias->insert(vias->begin()+idx, new cvec4d(pos));
}

inline void Spline::planPath(Trajectory* trajectory, uint32_t timestep) {
  for(uint32_t i = 0; i < vias->size(); i ++) {
    cvec4d* cvia = vias->at(i);
    cvia->println();
    
    Setpoint* sp = new Setpoint(0, true);
    sp->time = 1000;
    for(int x = 0; x < (*(vias->end()))->x; x += 0.1) {
      sp->time = sp->time + 100;
      //sp->cpos->set(x,y,z,phi);
      // TODO cvel, cacc...
    }
  }
}

#endif
